#include "Collision.h"

Box::Box(float _x, float _y, float _w, float _h, float _vx, float _vy)
{
	this->x = _x;
	this->y = _y;
	this->w = _w;
	this->h = _h;
	this->vx = _vx;
	this->vy = _vy;
}
Box::Box(float _x, float _y, float _w, float _h)
{
	this->x = _x;
	this->y = _y;
	this->w = _w;
	this->h = _h;
	this->vx = 0;
	this->vy = 0;
}

// returns a box the spans both a current box and the destination box
Box Collision::GetSweptBroadphaseBox(Box b)
{
	Box broadphasebox(0.0f, 0.0f, 0.0f, 0.0f);

	broadphasebox.x = b.vx > 0 ? b.x : b.x + b.vx;
	broadphasebox.y = b.vy > 0 ? b.y : b.y + b.vy;
	broadphasebox.w = b.vx > 0 ? b.vx + b.w : b.w - b.vx;
	broadphasebox.h = b.vy > 0 ? b.vy + b.h : b.h - b.vy;

	return broadphasebox;
}

Box *Collision::getSweptBroadphaseBox(Box *b)
{


	float x = b->vx > 0 ? b->x : b->x + b->vx;
	float y = b->vy > 0 ? b->y : b->y + b->vy;
	float w = b->vx > 0 ? b->vx + b->w : b->w - b->vx;
	float h = b->vy > 0 ? b->vy + b->h : b->h - b->vy;

	Box *broadphasebox = new Box(x, y, w, h);
	return broadphasebox;
}

// returns true if the boxes are colliding (velocities are not used)
bool Collision::AABBCheck(Box b1, Box b2)
{
	return !(b1.x + b1.w < b2.x || b1.x > b2.x + b2.w || b1.y + b1.h < b2.y || b1.y > b2.y + b2.h);
}

bool Collision::RectVsRect(Box b1, Box b2)
{
	return (b1.x < b2.x + b2.w && b1.x + b1.w > b2.x && b1.y < b2.y + b2.h && b1.y + b1.h > b2.y);
	//return !(b1.x + b1.w < b2.x || b1.x > b2.x + b2.w || b1.y + b1.h < b2.y || b1.y > b2.y + b2.h);
}

float Collision::SweptAABB(Box b1, Box b2, float &normalx, float &normaly)
{
	float xInvEntry, yInvEntry;
	float xInvExit, yInvExit;

	// find the distance between the objects on the near and far sides for both x and y
	if (b1.vx > 0.0f)
	{
		xInvEntry = b2.x - (b1.x + b1.w);
		xInvExit = (b2.x + b2.w) - b1.x;
	}
	else 
	{
		xInvEntry = (b2.x + b2.w) - b1.x;
		xInvExit = b2.x - (b1.x + b1.w);
	}

	if (b1.vy > 0.0f)
	{
		yInvEntry = b2.y - (b1.y + b1.h);
		yInvExit = (b2.y + b2.h) - b1.y;
	}
	else
	{
		yInvEntry = (b2.y + b2.h) - b1.y;
		yInvExit = b2.y - (b1.y + b1.h);
	}

	// find time of collision and time of leaving for each axis (if statement is to prevent divide by zero)
	float xEntry, yEntry;
	float xExit, yExit;

	if (b1.vx == 0.0f)
	{
		xEntry = -std::numeric_limits<float>::infinity();
		xExit = std::numeric_limits<float>::infinity();
	}
	else
	{
		xEntry = xInvEntry / b1.vx;
		xExit = xInvExit / b1.vx;
	}

	if (b1.vy == 0.0f)
	{
		yEntry = -std::numeric_limits<float>::infinity();
		yExit = std::numeric_limits<float>::infinity();
	}
	else
	{
		yEntry = yInvEntry / b1.vy;
		yExit = yInvExit / b1.vy;
	}

	// find the earliest/latest times of collision
	float entryTime = std::max(xEntry, yEntry);
	float exitTime = std::min(xExit, yExit);

	// if there was no collision
	if (entryTime > exitTime || xEntry < 0.0f && yEntry < 0.0f || xEntry > 1.0f || yEntry > 1.0f)
	{
		normalx = 0.0f;
		normaly = 0.0f;
		return 1.0f;
	}
	else // if there was a collision
	{        		
		// calculate normal of collided surface
		if (xEntry > yEntry)
		{
			if (xInvEntry < 0.0f)
			{
				normalx = 1.0f;
				normaly = 0.0f;
			}
			else
			{
				normalx = -1.0f;
				normaly = 0.0f;
			}
		}
		else
		{
			if (yInvEntry < 0.0f)
			{
				normalx = 0.0f;
				normaly = 1.0f;
			}
			else
			{
				normalx = 0.0f;
				normaly = -1.0f;
			}
		}

		// return the time of collision
		return entryTime;
	}
}

bool Collision::Deflect(Box box, Box block)
{
	float normalx = 0;
	float normaly = 0;
	float collsion_time = SweptAABB(box, block, normalx, normaly);
	if (0 < collsion_time && collsion_time < 1)
	{
		box.x += box.vx * collsion_time;
		box.y += box.vy * collsion_time;
		float remaining_time = 1.0f - collsion_time;

		//deflect
		box.vx *= remaining_time;
		box.vy *= remaining_time;
		if (abs(normalx) > 0.0001f)
		{
			box.vx = -box.vx;
		}
		if (abs(normaly) > 0.0001f)
		{
			box.vy = -box.vy;
		}
		return true;
	}
	return 0;
}

//bool Collision::RectVsRect(RECT r1, RECT r2)
//{
//	return (r1.left<r2.right&&r1.right>r2.left&&r1.top<r2.bottom&&r1.bottom>r2.top);
//}